#include "class_timer.hpp"
#include "class_detector.h"
#include "serialPort.hpp"
#include "find_color.hpp"
#include <memory>
#include <thread>


char s_buff[10];       //发送缓冲区
char r_buff[4] = {0x00,0x00,0x00,0x00};         //接收缓冲区
int cls = -1;           //垃圾种类     
int color = -1;           //垃圾种类           
int command = 1;        //模式选择
const char *dev  = "/dev/ttyTHS1";      //串口

serialPort myserial;        
std::unique_ptr<Detector> detector(new Detector());


/***************************************************************
  *  @brief     延时
  *  @param     int time ：单位ms
 **************************************************************/
void Delay(int time)//time*1000为秒数 
{ 
    clock_t now = clock(); 
    while(clock()-now<time); 
}

/***************************************************************
  *  @brief     多线程接收串口
  *  @param     无
 **************************************************************/
void thread_read()      //串口接收
{
    int nread;
    while(1)
    {
        nread = myserial.readBuffer(r_buff, 4);     //接收四个字符
        if (nread > 0)              
        {
            if (r_buff[0] == 0x55 and r_buff[3] == 0xaa)    //判断帧头帧尾
            {
                if(r_buff[1] == 0xb0)       //找垃圾
                {
                    command = 1;
                    std::cout << "0xb0" << std::endl;
                } 
                else if(r_buff[1] == 0xb1)      //找颜色块
                {
                    command = 2;
                    if (r_buff[2] == 0xa0)
                        cls = 0;
                    else if(r_buff[2] == 0xa1)
                        cls = 1;
                    else if(r_buff[2] == 0xa2)
                        cls = 2;
                    else if(r_buff[2] == 0xa3)
                        cls = 3;
                    else if(r_buff[2] == 0xa4)
                        cls = 4;
                    std::cout << "0xb1" << std::endl;
                }
                else if(r_buff[1] == 0xb2)
                {
                    command = 0;
                    std::cout << "0xb2" << std::endl;
                }
            }
        }
    }
}

/***************************************************************
  *  @brief     封装数据帧
  *  @param     char* s_buff ：发送缓冲区
                int data ：待发送数据
                int id ：垃圾种类
                int com ：是否发送捡垃圾指令
 **************************************************************/
void type_turn(char* s_buff,int x,int y,int id,int com)        //封装数据帧
{
    if(x<=999 && x>=100)
    {
        s_buff[1] = x/100 + '0';
        s_buff[2] = (x%100)/10 + '0';
        s_buff[3] = x%10 + '0';
    }
    else if(x>=10)
    {
        s_buff[1] = '0';
        s_buff[2] = x/10 + '0';
        s_buff[3] = x%10 + '0';
    }
    else
    {
        s_buff[1] = '0';
        s_buff[2] = '0';
        s_buff[3] = x%10 + '0';
    }

    if(y<=999 && y>=100)
    {
        s_buff[4] = y/100 + '0';
        s_buff[5] = (y%100)/10 + '0';
        s_buff[6] = y%10 + '0';
    }
    else if(y>=10)
    {
        s_buff[4] = '0';
        s_buff[5] = y/10 + '0';
        s_buff[6] = y%10 + '0';
    }
    else
    {
        s_buff[4] = '0';
        s_buff[5] = '0';
        s_buff[6] = y%10 + '0';
    }

    if (com == 1)   //捡取命令
        s_buff[8] = 0xcc;
    else if (com == 2)
        s_buff[8] = 0xdd;
    else
        s_buff[8] = 0x00;
    s_buff[7] = id + '0';
    s_buff[0] = 0x55;s_buff[9] = 0xaa;
}


/***************************************************************
  *  @brief     检测垃圾
  *  @param     cv::VideoCapture capture ：视频
 **************************************************************/
int detect_(cv::VideoCapture capture)       
{
    Timer timer;
    cv::Mat image0;
    int counts = 0;
    int last_y = -1,last_id = -1,last_center = -1;
    for (;;)
    {
        if (command != 1)
        {
            cv::destroyAllWindows();   //释放全部窗口
            break;
        } 

        BatchResult res;
        capture >> image0;            //读取当前帧
        
        if(!image0.empty())          //判断输入的视频帧是否为空的
        {          
            detect
            timer.reset();
            detector->detect(image0, res);
            timer.out("detect");

            //display
            int bottom_y = 0,center = 0,distance = 0,last_dis=999,j = 0,index = -1;
            int id = -1;
            
            for (const auto &r : res)       //循环遍历每个框
            {

                if (r.rect.height*r.rect.width <= 150)          //滤除噪声
                {
                    std::cout << "error" << std::endl;  
                    j++;
                    continue;
                }
                cv::rectangle(image0, r.rect, cv::Scalar(255, 0, 0), 2);        //目标矩形框
                std::stringstream stream;
                stream << std::fixed << std::setprecision(2) << "id:" << r.id << "  score:" << r.prob;  
                cv::putText(image0, stream.str(), cv::Point(r.rect.x, r.rect.y - 5), 0, 0.5, cv::Scalar(0, 0, 255), 2); //类别，置信度
            
                bottom_y = r.rect.y + r.rect.height;      //框底部y坐标
                distance = fabs(480 - bottom_y);          //计算框下边沿距离画面下边沿的距离
                if (distance < last_dis)            //取最小距离
                {
                    last_dis = distance;
                    index = j;
                }
                j++;
            }    

            if(index != -1)     //视野中有目标才执行
            {
                center = res[index].rect.x + res[index].rect.width/2;  //中心x坐标
                id = res[index].id;         //类别
                bottom_y = res[index].rect.y + res[index].rect.height;      //下边沿y坐标
                
                if ((id != last_id) && (last_id != -1))      //前后两帧目标不一致，跳过（第一帧不判断）
                {
                    counts++;
                    if (counts >= 2)       //连续3帧跳过，丢失目标
                    {
                        counts = 0;
                        last_id = -1;
                        last_y = -1;
                        last_center = -1;
                    }       
                    continue;
                }
                else            //没有连续丢掉目标
                {
                    if (counts > 0)
                        counts--;
                }

                last_id = id;            //记录上次的id
                last_y = bottom_y;      //记录上次的y坐标
                last_center = center;   //记录上次发送的x坐标

                type_turn(s_buff,center,bottom_y,id,1);      ///封装数据帧，发送最近的框中心坐标和对应类别,1对应0xcc
                myserial.writeBuffer(s_buff, 10);             //发送
                std::cout << "x:" << center << "y" << bottom_y << "id:" << id << std::endl;  
            }
            else
            {
                Delay(10);          //10ms
                type_turn(s_buff,0,0,-1,0);      ///封装数据帧
                myserial.writeBuffer(s_buff, 10);             //无目标。发送0，种类-1
                std::cout << "none"  << std::endl;  
            }
            cv::imshow("image", image0);
            if(cv::waitKey(10) == 'q')   //延时20ms,获取用户是否按键的情况，如果按下q，会退出程序 
                return 0;
        }
    }	
}

/***************************************************************
  *  @brief     颜色识别
  *  @param     cv::VideoCapture capture ：视频
 **************************************************************/
void color_detect(cv::VideoCapture capture)
{
    cv::Mat image;
    while(1)             //颜色识别
    {
        if (command != 2)
        {
            cv::destroyAllWindows();   //释放全部窗口
            break;
        }
                     
        int center_x;
        capture >> image;            //读取当前帧
        if(!image.empty())          //判断输入的视频帧是否为空的
        {    
            center_x = find_goal(image,cls); 

            type_turn(s_buff,center_x,0,cls,2);      ///封装数据帧，发送距中心最近的框中心坐标和对应类别
            myserial.writeBuffer(s_buff, 10);             //发送
            std::cout << center_x << "       " << cls << std::endl;  
            if(cv::waitKey(1) == 'q')   //延时20ms,获取用户是否按键的情况，如果按下q，会推出程序 
                break;
        }
        Delay(10);
    }
}


/***************************************************************
  *  @brief     主函数
  *  @param     无
 **************************************************************/
int main()
{
	Config config_v4_tiny;
	config_v4_tiny.net_type = YOLOV4_TINY;
	config_v4_tiny.detect_thresh = 0.5;
	config_v4_tiny.file_model_cfg = "../configs/yolov4-tiny.cfg";
	config_v4_tiny.file_model_weights = "../configs/yolov4-tiny.weights";
	config_v4_tiny.calibration_image_list_file_txt = "../configs/calibration_images.txt";
	config_v4_tiny.inference_precison = FP32;

	detector->init(config_v4_tiny);				//传入设置参数

    // cv::Mat image0;
    cv::VideoCapture capture(0);                //读取视摄像头实时画面数据
	capture.set(3, 640);//宽度 
	capture.set(4, 480);//高度

    int i,nwrite;
    std::cout<<"serialPort Test"<< std::endl;

    myserial.OpenPort(dev);     //打开串口
    myserial.setup(115200,0,8,1,'N');   //设置波特率，校验位，停止位

    std::thread send_data(thread_read);     //多线程监控串口   
    send_data.detach();

    while(1)
    {
        if (command == 1)          //找垃圾
            detect_(capture);      //检测并发送坐标
        else if(command == 2)      //找色块
            color_detect(capture);
        else if(command == 0)      //空闲模式
        {
            Delay(10);
            continue;
        }
    }

    myserial.ClosePort();   //关闭串口
    capture.release();     //释放摄像头资源
    cv::destroyAllWindows();   //释放全部窗口
}
